//C头文件
#include <iostream>  
//ros头文件
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "nav_msgs/Path.h"
#include <tf/transform_broadcaster.h>
#include <ros/time.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/JointState.h>
#include <nav_msgs/Odometry.h>
//Eigen库头文件
#include "Eigen/Eigen"
//自己的头文件
#include "AP_InertialSensor_PureStrap.h"
#include "AP_AbsPosition_GPS.h"
#include "AP_SensorFusion_EKF.h"
#include "AP_SensorFusion_Initial.h"
#include "AP_Velocity_GPS.h"
#include "AP_Velocity_Odom.h"

#define Hz 0.005

using namespace std;
using namespace Eigen;

//暂且先默认里程计坐标系和IMU坐标系重合，并且初始时刻手动使其X轴指向正北

//惯导系统初始姿态/位置/速度
double Att_Lepo[3];
double Vel_Lepo[3];
double Pos_Lepo[3];

//惯性传感器标定误差
double IMU_Misa[12];
double IMU_Bias[6];

double IMU_Mean[6];

VectorXd INS_State(19);
MatrixXd PK(19, 19);

//惯性传感器读数
double IMU_Data[6];

VectorXd GPS_Data(3);
VectorXd GPS_Nois(3);

VectorXd GPS_Velo(3);
VectorXd GVelo_Nois(3);

VectorXd Odom_VeloB(3);//机体系下的里程计速度
VectorXd Odom_Velo(3);//导航系下的里程计速度
VectorXd Odom_Nois(3);

//初始化位置计数值
int init_Pos_num = 0;
int cnt_GPS = 0;

double flag = 0;	//量测标志

double Atti[3];		//输出姿态角

ros::Publisher state_pub;		//导航信息发布器
sensor_msgs::JointState state;		//导航信息输出

//初始化函数
void initialize_state()
{
	for (int Cnt = 0; Cnt != 6; Cnt++)
	{
		IMU_Misa[Cnt] = 0;
		IMU_Misa[Cnt + 6] = 0;
	}

	for (int Cnt = 0; Cnt != 3; Cnt++)
	{
		IMU_Bias[Cnt] = IMU_Mean[Cnt];
		IMU_Bias[Cnt + 3] = IMU_Mean[Cnt + 3];
	}

	Att_Lepo[0] = 0;
	Att_Lepo[1] = 0;
	Att_Lepo[2] = 0;

	Vel_Lepo[0] = 0;
	Vel_Lepo[1] = 0;
	Vel_Lepo[2] = 0;

	double rol = Att_Lepo[0] * pi / 180;
	double pit = Att_Lepo[1] * pi / 180;
	double yaw = Att_Lepo[2] * pi / 180;

	INS_State[0] = cos(yaw / 2)*cos(pit / 2)*cos(rol / 2) + sin(yaw / 2)*sin(pit / 2)*sin(rol / 2);
	INS_State[1] = cos(yaw / 2)*sin(pit / 2)*cos(rol / 2) + sin(yaw / 2)*cos(pit / 2)*sin(rol / 2);
	INS_State[2] = cos(yaw / 2)*cos(pit / 2)*sin(rol / 2) - sin(yaw / 2)*sin(pit / 2)*cos(rol / 2);
	INS_State[3] = -sin(yaw / 2)*cos(pit / 2)*cos(rol / 2) + cos(yaw / 2)*sin(pit / 2)*sin(rol / 2);

	for (int Cnt = 0; Cnt != 3; Cnt++){
		INS_State[Cnt + 4] = Vel_Lepo[Cnt];
	}

	INS_State[7] = Pos_Lepo[0] * pi / 180;
	INS_State[8] = Pos_Lepo[1] * pi / 180;
	INS_State[9] = Pos_Lepo[2];

	for (int Cnt = 0; Cnt != 9; Cnt++){
		INS_State[Cnt + 10] = 0;          //rad/s
	}

	//滤波器初始化
	AP_SensorFusion_Initial Filter_Ini(Pos_Lepo);
	Filter_Ini.Filter_Initial();

	PK = Filter_Ini.PK;

	for (int i = 0; i != 3; i++)
		Atti[i] = Att_Lepo[i];

	//量测噪声初始化
	GPS_Nois << 0.008, 0.008, 0.008;
	GVelo_Nois << 0.008, 0.008, 0.008;
	Odom_Nois << 0.001, 0.001, 0.001;//?

	state.effort.resize(22);
}

//回调函数--拿GPS量测数据
void Get_GPS(const sensor_msgs::JointState::ConstPtr& msg)//(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
{
	//GPS数据
	GPS_Data[0] = msg->effort[0];
	GPS_Data[1] = msg->effort[1];
	GPS_Data[2] = msg->effort[2];
	GPS_Velo[0] = msg->effort[3];
	GPS_Velo[1] = msg->effort[4];
	GPS_Velo[2] = 0;

	//用来初始化位置
	if (cnt_GPS < 5)
	{
		Pos_Lepo[0] = Pos_Lepo[0] + GPS_Data[0];
		Pos_Lepo[1] = Pos_Lepo[1] + GPS_Data[1];
		Pos_Lepo[2] = Pos_Lepo[2] + GPS_Data[2];
		cnt_GPS++;
	}
	
	flag = 1;
}

//回调函数--拿里程计量测数据
void Get_Odom(const nav_msgs::Odometry::ConstPtr& msg)//(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
{
	//变量定义
	double q0, q1, q2, q3;             //四元数
	double Tn2b[3][3];                 //导航系到机体系的姿态转移矩阵

	//用惯性递推得到的四元数
	q0 = INS_State[0];
	q1 = INS_State[1];
	q2 = INS_State[2];
	q3 = INS_State[3];

	//姿态转移矩阵解算
	Tn2b[0][0] = q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3;
	Tn2b[0][1] = 2 * (q1*q2 + q0*q3);
	Tn2b[0][2] = 2 * (q1*q3 - q0*q2);
	Tn2b[1][0] = 2 * (q1*q2 - q0*q3);
	Tn2b[1][1] = q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3;
	Tn2b[1][2] = 2 * (q2*q3 + q0*q1);
	Tn2b[2][0] = 2 * (q1*q3 + q0*q2);
	Tn2b[2][1] = 2 * (q2*q3 - q0*q1);
	Tn2b[2][2] = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;

	//里程计数据
	Odom_VeloB[0] = 0;
	Odom_VeloB[1] = msg->twist.twist.linear.x;
	Odom_VeloB[2] = 0;
	
	//导航系下的里程计速度输出
	Odom_Velo[0] = Tn2b[0][0] * Odom_VeloB[0] + Tn2b[1][0] * Odom_VeloB[1] + Tn2b[2][0] * Odom_VeloB[2];
	Odom_Velo[1] = Tn2b[0][1] * Odom_VeloB[0] + Tn2b[1][1] * Odom_VeloB[1] + Tn2b[2][1] * Odom_VeloB[2];
	Odom_Velo[2] = Tn2b[0][2] * Odom_VeloB[0] + Tn2b[1][2] * Odom_VeloB[1] + Tn2b[2][2] * Odom_VeloB[2];

	flag = 2;
}

//回调函数--预测与融合
void PureStrapAndFusion(const sensor_msgs::Imu::ConstPtr& msg)
{
	//IMU数据
	IMU_Data[0] = -msg->linear_acceleration.y;
	IMU_Data[1] = msg->linear_acceleration.x;
	IMU_Data[2] = msg->linear_acceleration.z;
	IMU_Data[3] = -msg->angular_velocity.y;
	IMU_Data[4] = msg->angular_velocity.x;
	IMU_Data[5] = msg->angular_velocity.z;

	if (init_Pos_num < 1000)
	{
		for (int i = 0; i != 6; i++){
			IMU_Mean[i] = IMU_Mean[i] + IMU_Data[i];
		}		

		init_Pos_num++;
		return;
	}
	if (init_Pos_num == 1000) 
	{
		Pos_Lepo[0] = Pos_Lepo[0] / cnt_GPS;//求初始位置
		Pos_Lepo[1] = Pos_Lepo[1] / cnt_GPS;
		Pos_Lepo[2] = Pos_Lepo[2] / cnt_GPS;

		for (int i = 0; i != 6; i++){
			IMU_Mean[i] = IMU_Mean[i] / init_Pos_num;
		}

		//初始化
		initialize_state();
		init_Pos_num = 2000;
		cout << cnt_GPS << endl;
	}

	AP_InertialSensor_PureStrap IMU_Pstr(IMU_Data, IMU_Misa, IMU_Bias, Hz, INS_State, PK, Atti);
	IMU_Pstr.Inertial_Solut();

	VectorXd Measure_Correct, Measure_Pred, Noise;
	MatrixXd ObserMatrix;
	int dimen;

	if (flag == 1)
	{
		AP_AbsPosition_GPS GPS_Meas(GPS_Data, 3, IMU_Pstr.State_Pred);
		GPS_Meas.Observation_Form();

		AP_Velocity_GPS GPS_Velocity(GPS_Velo, 3, IMU_Pstr.State_Pred);
		GPS_Velocity.Observation_Form();

		//开辟合成矩阵
		VectorXd Synth_MC(6), Synth_MP(6), Synth_NO(6);
		MatrixXd Sybth_OB(6, 19);
		Synth_MC.block(0, 0, 3, 1) = GPS_Velocity.Measure_Correct;
		Synth_MC.block(3, 0, 3, 1) = GPS_Meas.Measure_Correct;
		Synth_MP.block(0, 0, 3, 1) = GPS_Velocity.Measure_Pred;
		Synth_MP.block(3, 0, 3, 1) = GPS_Meas.Measure_Pred;
		Synth_NO.block(0, 0, 3, 1) = GVelo_Nois;
		Synth_NO.block(3, 0, 3, 1) = GPS_Nois;
		Sybth_OB.block(0, 0, 3, 19) = GPS_Velocity.ObserMatrix;
		Sybth_OB.block(3, 0, 3, 19) = GPS_Meas.ObserMatrix;

		dimen = 6;
		Measure_Correct = Synth_MC;
		Measure_Pred = Synth_MP;
		Noise = Synth_NO;
		ObserMatrix = Sybth_OB;

		AP_SensorFusion_EKF EKF_Fusin(IMU_Pstr.State_Pred, Measure_Correct, ObserMatrix, Measure_Pred, Noise, dimen, IMU_Pstr.PK_Pred);
		EKF_Fusin.EKF_Solut();

		INS_State = EKF_Fusin.State;
		PK = EKF_Fusin.PK;

		for (int i = 0; i != 3; i++)
			Atti[i] = EKF_Fusin.AttiN[i];

		flag = 0;
	}
	else if (flag == 2)
	{
		AP_Velocity_Odom Odom_Velocity(Odom_Velo, 3, IMU_Pstr.State_Pred);
		Odom_Velocity.Observation_Form();

		dimen = 3;
		Measure_Correct = Odom_Velocity.Measure_Correct;
		Measure_Pred = Odom_Velocity.Measure_Pred;
		Noise = Odom_Nois;
		ObserMatrix = Odom_Velocity.ObserMatrix;

		AP_SensorFusion_EKF EKF_Fusin(IMU_Pstr.State_Pred, Measure_Correct, ObserMatrix, Measure_Pred, Noise, dimen, IMU_Pstr.PK_Pred);
		EKF_Fusin.EKF_Solut();

		INS_State = EKF_Fusin.State;
		PK = EKF_Fusin.PK;

		for (int i = 0; i != 3; i++)
			Atti[i] = EKF_Fusin.AttiN[i];

		flag = 0;
	}
	else
	{
		for (int i = 0; i != 3; i++)
			Atti[i] = IMU_Pstr.AttiN[i];
		INS_State = IMU_Pstr.State_Pred;
		PK = IMU_Pstr.PK_Pred;
	}

	for (int i = 0; i != 19; i++)
	{
		state.effort[i] = INS_State[i];
	}
	for (int i = 0; i != 3; i++)
	{
		state.effort[i + 19] = Atti[i];
	}
	//发布消息
	state_pub.publish(state);
}



int main(int argc, char** argv)
{

	//ros初始化
	ros::init(argc, argv, "kalman_filter_node");
	ros::NodeHandle n;

	state_pub = n.advertise<sensor_msgs::JointState>("state", 1000);

	ros::Subscriber attitude_sub = n.subscribe("imu/data", 100, PureStrapAndFusion);
	ros::Subscriber gps_sub = n.subscribe("chattergps", 100, Get_GPS);
	ros::Subscriber state_sub = n.subscribe("odom", 100, Get_Odom);

	ros::spin();
	return 0;

}
